#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from std_msgs.msg import String, Float32MultiArray
import cv2
import mediapipe as mp
import numpy as np

class GestureRecognizer(Node):
    def __init__(self):
        super().__init__('gesture_recognizer')
        
        # 初始化MediaPipe
        self.mp_hands = mp.solutions.hands
        self.hands = self.mp_hands.Hands(
            static_image_mode=False,
            max_num_hands=2,
            min_detection_confidence=0.7,
            min_tracking_confidence=0.5)
        self.mp_drawing = mp.solutions.drawing_utils
        
        # ROS2设置
        self.bridge = CvBridge()
        self.subscription = self.create_subscription(
            Image,
            '/camera/image_raw',
            self.image_callback,
            10)
        self.gesture_pub = self.create_publisher(String, '/gesture', 10)
        self.landmarks_pub = self.create_publisher(Float32MultiArray, '/hand_landmarks', 10)
        
        # 手势定义
        self.GESTURES = {
            "fist": [0, 1, 1, 1, 1],       # 握拳
            "thumb_up": [1, 0, 0, 0, 0],    # 点赞
            "thumb_down": [0, 0, 0, 0, 1],   # 倒赞
            "victory": [0, 1, 1, 0, 0],      # 剪刀手
            "palm": [1, 1, 1, 1, 1],        # 手掌
            "point": [0, 1, 0, 0, 0]        # 食指指
        }
        
        self.get_logger().info("手势识别节点已启动，等待图像输入...")

    def image_callback(self, msg):
        try:
            # 转换为OpenCV格式
            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
            rgb_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
            
            # MediaPipe处理
            results = self.hands.process(rgb_image)
            
            if results.multi_hand_landmarks:
                for hand_landmarks in results.multi_hand_landmarks:
                    # 绘制手部关键点
                    self.mp_drawing.draw_landmarks(
                        cv_image, hand_landmarks, self.mp_hands.HAND_CONNECTIONS)
                    
                    # 获取手势特征
                    gesture = self.detect_gesture(hand_landmarks)
                    
                    # 发布识别结果
                    gesture_msg = String()
                    gesture_msg.data = gesture
                    self.gesture_pub.publish(gesture_msg)
                    
                    # 发布关键点坐标（归一化值）
                    landmarks_msg = Float32MultiArray()
                    for landmark in hand_landmarks.landmark:
                        landmarks_msg.data.extend([landmark.x, landmark.y, landmark.z])
                    self.landmarks_pub.publish(landmarks_msg)
                    
                    # 在图像上标注手势
                    cv2.putText(cv_image, gesture, (50, 50), 
                                cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
            
            # 显示结果
            cv2.imshow('Gesture Recognition', cv_image)
            cv2.waitKey(1)
            
        except Exception as e:
            self.get_logger().error(f"处理图像时出错: {str(e)}")

    def detect_gesture(self, landmarks):
        """核心手势识别逻辑"""
        # 获取指尖坐标
        tips = [4, 8, 12, 16, 20]  # 拇指、食指、中指、无名指、小指尖
        tip_positions = []
        
        for tip in tips:
            x = landmarks.landmark[tip].x
            y = landmarks.landmark[tip].y
            tip_positions.append((x, y))
        
        # 获取手掌根部坐标
        wrist = (landmarks.landmark[0].x, landmarks.landmark[0].y)
        
        # 计算各手指是否展开
        finger_states = []
        for i, tip in enumerate(tip_positions):
            # 与手掌根部比较Y坐标（简单实现）
            if tip[1] < wrist[1] - 0.05:  # 阈值可调整
                finger_states.append(1)  # 手指展开
            else:
                finger_states.append(0)  # 手指弯曲
        
        # 匹配预定义手势
        for gesture_name, pattern in self.GESTURES.items():
            if finger_states == pattern:
                return gesture_name
        
        return "unknown"

def main(args=None):
    rclpy.init(args=args)
    node = GestureRecognizer()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()
        cv2.destroyAllWindows()

if __name__ == '__main__':
    main()